# coding = utf-8

"""
处理感知输出结果
"""
import math
import carla

from agents.tools.misc import compute_magnitude_angle, positive
from agents.tools.misc import get_speed
from agents.map.local_map import LocalRoadTreeDeque

class PredictObstacle:
    def __init__(self):
        self._speed = None
        self._bbox = None
        self._center = None

class PerceptPrediction:
    def __init__(self, world, hero:carla.Vehicle):
        self._world = world
        self._hero = hero
        pass

    def traffic_light(self):
        """
        当有红绿灯时，在停止线上放置一个虚拟障碍物
        """
        pass

    def sort_static_obstacle(self, local_trace_tree, static_obs):
        """
        根据距离按序排布静止障碍物
        """
        static_dist_obs = [] #  (dist, obs)
        cur_hero_tf = self._hero.get_transform()
        for actor  in static_obs:
            tf = actor.get_transform()
            dist, angle = compute_magnitude_angle(tf.location, cur_hero_tf.location, cur_hero_tf.rotation.yaw)
            sign = math.cos(math.radians(angle))
            if sign < 0:
                dist *= -1.0
            static_dist_obs.append((dist, actor))
        return sorted(static_dist_obs)

    def sort_moving_obstacle(self, local_trace_tree, moving_obstacle):
        """
        根据ttc按序排布移动障碍物，实际仍然是按照距离排序，ttc需要考虑轨迹
        """
        moving_ttc_obs = [] # (ttc, obs)
        cur_hero_tf = self._hero.get_transform()
        # hero_speed = self._hero.get_velocity()
        for actor in moving_obstacle:
            # tf = actor.get_transform()
            # speed = actor.get_velocity()
            # diff_dist = tf.location - cur_hero_tf.location
            # diff_speed = speed - hero_speed
            # ttc = min(diff_dist.x / diff_speed.x, diff_dist.y / diff_speed.y) # 假定匀速模型
            tf = actor.get_transform()
            dist, angle = compute_magnitude_angle(tf.location, cur_hero_tf.location, cur_hero_tf.rotation.yaw)
            sign = math.cos(math.radians(angle))
            if sign < 0:
                dist *= -1.0
            moving_ttc_obs.append((dist, actor))
        return sorted(moving_ttc_obs)

    def run_step(self, local_trace_tree, static_obs, moving_obs):
        """
        TODO 之后可以根据 s距离、l距离来过滤数据
        """
        self.traffic_light() # 先不用
        static_dist_obs = self.sort_static_obstacle(local_trace_tree, static_obs)
        moving_ttc_obs = self.sort_moving_obstacle(local_trace_tree, moving_obs)
        return static_dist_obs, moving_ttc_obs